/* Viterbi decoder for arbitrary convolutional code
 * viterbi27 and viterbi37 for the r=1/2 and r=1/3 K=7 codes are faster
 * Copyright 1999 Phil Karn, KA9Q
 * May be used under the terms of the GNU Public License
 */

#include "kiwi.h"

#ifndef KIWI

#undef K

#define _XOPEN_SOURCE 1

/* Select code here */
#define	K 7			/* Constraint length */
#define N 2			/* Number of symbols per data bit */
#define Polys	Poly27		/* Select polynomials here */
//#define Polys	Poly47		/* Select polynomials here */

/* Rate 1/2 codes */
int Poly23[] = { 0x7, 0x5 };		/* K=3 */
int Poly24[] = { 0xf, 0xb };		/* k=4 */
int Poly25[] = { 0x17, 0x19 };		/* k=5 */
int Poly25a[] = { 0x13, 0x1b };	/* k=5, used in GSM?  */
int Poly26[] = { 0x2f, 0x35 };		/* k=6  */

//int Poly27[] = { 0x6d, 0x4f };		/* k=7; very popular with NASA and industry  */
int Poly27[] = { 0x4f, -0x6d };		/* k=7; Galileo E1B; "-" means G2 inverted  */

int Poly28[] = { 0x9f, 0xe5 };		/* k=8  */
int Poly29[] = { 0x1af, 0x11d };	/* k=9; used in IS-95 CDMA  */
int Poly215[] = { 0x45dd, 0x69e3 };	/* k = 15  */

/* Rate 1/3 codes */
int Poly33[] = { 0x7, 0x7, 0x5 };	/* k = 3 */
int Poly34[] = { 0xf, 0xb, 0xd };	/* k = 4 */
int Poly35[] = { 0x1f, 0x1b, 0x15 };	/* k = 5 */
int Poly36[] = { 0x2f, 0x35, 0x39 };	/* k = 6 */
int Poly37[] = { 0x4f, 0x57, 0x6d };	/* k = 7; also popular with NASA and industry  */
//int Poly38[] = { 0xef, 0x9b, 0xa9 };	/* k = 8  */
int Poly38[] = { 0x95, 0xd9, 0xf7 };	/* k = 8  */
int Poly39[] = { 0x1ed, 0x19b, 0x127 }; /* k = 9; used in IS-95 CDMA  */

/* Rate 1/4 codes */
int Poly47[] = { 0x6d, 0x4f, 0x53, 0x6d };	/* k = 7; DAB  */   //<=== bit rev of 5b 79 65 5b
//int Poly47[] = { 0x5b, 0x79, 0x65, 0x5b };	/* k = 7; DAB  */

/* Rate 1/6 code */
int Poly615[] = { 042631, 047245, 073363, 056507, 077267, 064537 }; /* k=15 on Mars Pathfinder */

#define	OFFSET	128


#include <memory.h>
#include <stdio.h>
#include <stdlib.h>
#include <math.h>

/* There ought to be a more general way to do this efficiently ... */
#if defined(__alpha__) || defined(__x86_64__)
#define LONGBITS 64
#define LOGLONGBITS 6
#else
#define LONGBITS 32
#define LOGLONGBITS 5
#endif

#undef max
#define max(x,y) ((x) > (y) ? (x) : (y))
#define D       (1 << max(0,K-LOGLONGBITS-1))


/* 8-bit parity lookup table, generated by partab.c */
unsigned char Partab[] = {
 0, 1, 1, 0, 1, 0, 0, 1,
 1, 0, 0, 1, 0, 1, 1, 0,
 1, 0, 0, 1, 0, 1, 1, 0,
 0, 1, 1, 0, 1, 0, 0, 1,
 1, 0, 0, 1, 0, 1, 1, 0,
 0, 1, 1, 0, 1, 0, 0, 1,
 0, 1, 1, 0, 1, 0, 0, 1,
 1, 0, 0, 1, 0, 1, 1, 0,
 1, 0, 0, 1, 0, 1, 1, 0,
 0, 1, 1, 0, 1, 0, 0, 1,
 0, 1, 1, 0, 1, 0, 0, 1,
 1, 0, 0, 1, 0, 1, 1, 0,
 0, 1, 1, 0, 1, 0, 0, 1,
 1, 0, 0, 1, 0, 1, 1, 0,
 1, 0, 0, 1, 0, 1, 1, 0,
 0, 1, 1, 0, 1, 0, 0, 1,
 1, 0, 0, 1, 0, 1, 1, 0,
 0, 1, 1, 0, 1, 0, 0, 1,
 0, 1, 1, 0, 1, 0, 0, 1,
 1, 0, 0, 1, 0, 1, 1, 0,
 0, 1, 1, 0, 1, 0, 0, 1,
 1, 0, 0, 1, 0, 1, 1, 0,
 1, 0, 0, 1, 0, 1, 1, 0,
 0, 1, 1, 0, 1, 0, 0, 1,
 0, 1, 1, 0, 1, 0, 0, 1,
 1, 0, 0, 1, 0, 1, 1, 0,
 1, 0, 0, 1, 0, 1, 1, 0,
 0, 1, 1, 0, 1, 0, 0, 1,
 1, 0, 0, 1, 0, 1, 1, 0,
 0, 1, 1, 0, 1, 0, 0, 1,
 0, 1, 1, 0, 1, 0, 0, 1,
 1, 0, 0, 1, 0, 1, 1, 0,
};

int Rate = N;

int Syms[1 << K];
int VDInit = 0;






// metrics.c

//double erf(double x);

//#define M_SQRT2 1.4142135623730950488016887242097
//#define M_LOG2E 2.7182818284590452353602874713527

/* Normal function integrated from -Inf to x. Range: 0-1 */
#define	normal(x)	(0.5 + 0.5*erf((x)/M_SQRT2))

/* Logarithm base 2 */
#define	log2(x)	(log(x)*M_LOG2E)

/* Generate log-likelihood metrics for 8-bit soft quantized channel
 * assuming AWGN and BPSK
 */
int
gen_met(
int mettab[2][256],	/* Metric table, [sent sym][rx symbol] */
int amp,		/* Signal amplitude, units */
double noise,		/* Relative noise voltage */
double bias,		/* Metric bias; 0 for viterbi, rate for sequential */
int scale		/* Scale factor */
){
//  double n;
  int s,bit;
  double metrics[2][256];
  double p0,p1;

  /* Zero is a special value, since this sample includes all
   * lower samples that were clipped to this value, i.e., it
   * takes the whole lower tail of the curve
   */
  p1 = normal(((0-OFFSET+0.5)/amp - 1)/noise);	/* P(s|1) */

  /* Prob of this value occurring for a 0-bit */	/* P(s|0) */
  p0 = normal(((0-OFFSET+0.5)/amp + 1)/noise);
  metrics[0][0] = log2(2*p0/(p1+p0)) - bias;
  metrics[1][0] = log2(2*p1/(p1+p0)) - bias;

  for(s=1;s<255;s++){
    /* P(s|1), prob of receiving s given 1 transmitted */
    p1 = normal(((s-OFFSET+0.5)/amp - 1)/noise) -
      normal(((s-OFFSET-0.5)/amp - 1)/noise);

    /* P(s|0), prob of receiving s given 0 transmitted */
    p0 = normal(((s-OFFSET+0.5)/amp + 1)/noise) -
      normal(((s-OFFSET-0.5)/amp + 1)/noise);

#ifdef notdef
    printf("P(%d|1) = %lg, P(%d|0) = %lg\n",s,p1,s,p0);
#endif
    metrics[0][s] = log2(2*p0/(p1+p0)) - bias;
    metrics[1][s] = log2(2*p1/(p1+p0)) - bias;
  }
  /* 255 is also a special value */
  /* P(s|1) */
  p1 = 1 - normal(((255-OFFSET-0.5)/amp - 1)/noise);
  /* P(s|0) */
  p0 = 1 - normal(((255-OFFSET-0.5)/amp + 1)/noise);

  metrics[0][255] = log2(2*p0/(p1+p0)) - bias;
  metrics[1][255] = log2(2*p1/(p1+p0)) - bias;
#ifdef	notdef
  /* The probability of a raw symbol error is the probability
   * that a 1-bit would be received as a sample with value
   * 0-128. This is the offset normal curve integrated from -Inf to 0.
   */
  printf("symbol Pe = %lg\n",normal(-1/noise));
#endif
  for(bit=0;bit<2;bit++){
    for(s=0;s<256;s++){
      /* Scale and round to nearest integer */
      mettab[bit][s] = (int)floor(metrics[bit][s] * scale + 0.5);
#ifdef	notdef
      printf("metrics[%d][%d] = %lg, mettab = %d\n",
	     bit,s,metrics[bit][s],mettab[bit][s]);
#endif
    }
  }
  return 0;
}


// end_of_metrics.c


/* error function in double precision
double erf(double x)
{
    int k;
    double w, t, y;
    static double a[65] = {
        5.958930743e-11, -1.13739022964e-9,
        1.466005199839e-8, -1.635035446196e-7,
        1.6461004480962e-6, -1.492559551950604e-5,
        1.2055331122299265e-4, -8.548326981129666e-4,
        0.00522397762482322257, -0.0268661706450773342,
        0.11283791670954881569, -0.37612638903183748117,
        1.12837916709551257377,
        2.372510631e-11, -4.5493253732e-10,
        5.90362766598e-9, -6.642090827576e-8,
        6.7595634268133e-7, -6.21188515924e-6,
        5.10388300970969e-5, -3.7015410692956173e-4,
        0.00233307631218880978, -0.0125498847718219221,
        0.05657061146827041994, -0.2137966477645600658,
        0.84270079294971486929,
        9.49905026e-12, -1.8310229805e-10,
        2.39463074e-9, -2.721444369609e-8,
        2.8045522331686e-7, -2.61830022482897e-6,
        2.195455056768781e-5, -1.6358986921372656e-4,
        0.00107052153564110318, -0.00608284718113590151,
        0.02986978465246258244, -0.13055593046562267625,
        0.67493323603965504676,
        3.82722073e-12, -7.421598602e-11,
        9.793057408e-10, -1.126008898854e-8,
        1.1775134830784e-7, -1.1199275838265e-6,
        9.62023443095201e-6, -7.404402135070773e-5,
        5.0689993654144881e-4, -0.00307553051439272889,
        0.01668977892553165586, -0.08548534594781312114,
        0.56909076642393639985,
        1.55296588e-12, -3.032205868e-11,
        4.0424830707e-10, -4.71135111493e-9,
        5.011915876293e-8, -4.8722516178974e-7,
        4.30683284629395e-6, -3.445026145385764e-5,
        2.4879276133931664e-4, -0.00162940941748079288,
        0.00988786373932350462, -0.05962426839442303805,
        0.49766113250947636708
    };
    static double b[65] = {
        -2.9734388465e-10, 2.69776334046e-9,
        -6.40788827665e-9, -1.6678201321e-8,
        -2.1854388148686e-7, 2.66246030457984e-6,
        1.612722157047886e-5, -2.5616361025506629e-4,
        1.5380842432375365e-4, 0.00815533022524927908,
        -0.01402283663896319337, -0.19746892495383021487,
        0.71511720328842845913,
        -1.951073787e-11, -3.2302692214e-10,
        5.22461866919e-9, 3.42940918551e-9,
        -3.5772874310272e-7, 1.9999935792654e-7,
        2.687044575042908e-5, -1.1843240273775776e-4,
        -8.0991728956032271e-4, 0.00661062970502241174,
        0.00909530922354827295, -0.2016007277849101314,
        0.51169696718727644908,
        3.147682272e-11, -4.8465972408e-10,
        6.3675740242e-10, 3.377623323271e-8,
        -1.5451139637086e-7, -2.03340624738438e-6,
        1.947204525295057e-5, 2.854147231653228e-5,
        -0.00101565063152200272, 0.00271187003520095655,
        0.02328095035422810727, -0.16725021123116877197,
        0.32490054966649436974,
        2.31936337e-11, -6.303206648e-11,
        -2.64888267434e-9, 2.050708040581e-8,
        1.1371857327578e-7, -2.11211337219663e-6,
        3.68797328322935e-6, 9.823686253424796e-5,
        -6.5860243990455368e-4, -7.5285814895230877e-4,
        0.02585434424202960464, -0.11637092784486193258,
        0.18267336775296612024,
        -3.67789363e-12, 2.0876046746e-10,
        -1.93319027226e-9, -4.35953392472e-9,
        1.8006992266137e-7, -7.8441223763969e-7,
        -6.75407647949153e-6, 8.428418334440096e-5,
        -1.7604388937031815e-4, -0.0023972961143507161,
        0.0206412902387602297, -0.06905562880005864105,
        0.09084526782065478489
    };

    w = x < 0 ? -x : x;
    if (w < 2.2) {
        t = w * w;
        k = (int) t;
        t -= k;
        k *= 13;
        y = ((((((((((((a[k] * t + a[k + 1]) * t +
            a[k + 2]) * t + a[k + 3]) * t + a[k + 4]) * t +
            a[k + 5]) * t + a[k + 6]) * t + a[k + 7]) * t +
            a[k + 8]) * t + a[k + 9]) * t + a[k + 10]) * t +
            a[k + 11]) * t + a[k + 12]) * w;
    } else if (w < 6.9) {
        k = (int) w;
        t = w - k;
        k = 13 * (k - 2);
        y = (((((((((((b[k] * t + b[k + 1]) * t +
            b[k + 2]) * t + b[k + 3]) * t + b[k + 4]) * t +
            b[k + 5]) * t + b[k + 6]) * t + b[k + 7]) * t +
            b[k + 8]) * t + b[k + 9]) * t + b[k + 10]) * t +
            b[k + 11]) * t + b[k + 12];
        y *= y;
        y *= y;
        y *= y;
        y = 1 - y * y;
    } else {
        y = 1;
    }
    return x < 0 ? -y : y;
}

*/




//inline int
int
parity(int x)
{
  x ^= (x >> 16);
  x ^= (x >> 8);
  return Partab[x & 0xff];
}


/* Convolutionally encode data into binary symbols */
int encode(
unsigned char *symbols,
unsigned char *data,
unsigned int nbytes,
unsigned int startstate,
unsigned int endstate)
{
  int i,j;
  unsigned int encstate = startstate;
  
  while(nbytes-- != 0){
    for(i=7;i>=0;i--){
      //encstate = (encstate << 1) | ((*data >> i) & 1);
      encstate = (encstate << 1) | (*data++ & 1);
      for(j=0;j<N;j++)
        *symbols++ = parity(encstate & abs(Polys[j])) ^ (Polys[j] < 0);
    }
    //data++;
  }
  /* Flush out tail */
  for(i=K-2;i>=0;i--){
    encstate = (encstate << 1) | ((endstate >> i) & 1);
    for(j=0;j<N;j++)
      *symbols++ = parity(encstate & abs(Polys[j])) ^ (Polys[j] < 0);
  }
  return 0;
}

/* Viterbi decoder */
int
k_viterbi(
unsigned int *metric,           /* Final path metric (returned value) */
unsigned char *data,	/* Decoded output data */
unsigned char *symbols,	/* Raw deinterleaved input symbols */
unsigned int nbits,	/* Number of output bits */
int mettab[2][256],	/* Metric table, [sent sym][rx symbol] */
unsigned int startstate,         /* Encoder starting state */
unsigned int endstate            /* Encoder ending state */
){
  int bitcnt = -(K-1);
  long m0,m1;
  int i,j,sym;
  int mets[1 << N];
  // msvc does not like this ... beware if nbits <>768
  unsigned long paths[(nbits+K-1)*D],*pp,mask;                    //!!!!!!!!!!!!!!!!!!!
  //  unsigned long paths[(768+K-1)*D];
  // unsigned long *pp,mask;
  long cmetric[1 << (K-1)],nmetric[1 << (K-1)];

  memset(paths,0,sizeof(paths));

  if(!VDInit){
    //printf("Viterbi Syms:\n");
    for(i=0;i<(1<<K);i++){
      sym = 0;
      int sym2 = 0;
      for(j=0;j<N;j++) {
	    sym = (sym << 1) + parity(i & Polys[j]);
	    sym2 = (sym2 << 1) + (parity(i & abs(Polys[j])) ^ (Polys[j] < 0));
	  }
      Syms[i] = sym2;
      //printf("%d|%d ", sym, sym2);
    }
    //printf("\n");
    VDInit++;
  }

  startstate &= ~((1<<(K-1)) - 1);
  endstate &= ~((1<<(K-1)) - 1);

  /* Initialize starting metrics */
  for(i=0;i< 1<<(K-1);i++)
    cmetric[i] = -999999;
  cmetric[startstate] = 0;

  pp = paths;
  for(;;){ /* For each data bit */
    /* Read input symbols and compute branch metrics */
    for(i=0;i< 1<<N;i++){
      mets[i] = 0;
      for(j=0;j<N;j++){
	mets[i] += mettab[(i >> (N-j-1)) & 1][symbols[j]];
      }
    }
    symbols += N;
    /* Run the add-compare-select operations */
    mask = 1;
    for(i=0;i< 1 << (K-1);i+=2){
      int b1,b2;

      b1 = mets[Syms[i]];
      nmetric[i] = m0 = cmetric[i/2] + b1;
      b2 = mets[Syms[i+1]];
      b1 -= b2;
      m1 = cmetric[(i/2) + (1<<(K-2))] + b2;
      if(m1 > m0){
	nmetric[i] = m1;
	*pp |= mask;
      }
      m0 -= b1;
      nmetric[i+1] = m0;
      m1 += b1;
      if(m1 > m0){
	nmetric[i+1] = m1;
	*pp |= mask << 1;
      }
      mask <<= 2;
      if(mask == 0){
	mask = 1;
	pp++;
      }
    }
    if(mask != 1){
      pp++;
    }
    if(++bitcnt == (int)nbits){
      *metric = nmetric[endstate];
      break;
    }
    memcpy(cmetric,nmetric,sizeof(cmetric));
  }

  /* Chain back from terminal state to produce decoded data */
  if(data == NULL)
    return 0;/* Discard output */
//  memset(data,0,(nbits+7)/8); /* round up in case nbits % 8 != 0 */
  memset(data,0,nbits); /* round up in case nbits % 8 != 0 */

  for(i=nbits-1;i >= 0;i--){
    pp -= D;
    if(pp[endstate >> LOGLONGBITS] & (1L << (endstate & (LONGBITS-1)))){
      endstate |= (1 << (K-1));
// output bitsstream rather than bytestream
//            data[i>>3] |= 0x80 >> (i&7);
            data[i] = 1;
    }
    endstate >>= 1;
  }
  return 0;
}

//-----
int mettab[2][256];

int init_viterbi()
{
    int amp = 1;
    double noise = 1.0;

    gen_met(mettab,amp,noise,0.,4);
    return 0;
}

int viterbi(char *logfile, unsigned char *ibuf, int ilen, unsigned char *obuf, int *olen)
{
    int i;
    unsigned int metric;
    int bits;
    int amp = 1;
    unsigned char symbols[3096];

     // ilen = 3096 = 3072 + 4*6
    bits = (ilen / N) - (K-1);
    // convert soft symbols
    for (i=0; i<ilen; i++)
    {
        // 1->127, 0->129, erased->128,
        symbols[i] = (ibuf[i]==1) ? OFFSET+amp : (ibuf[i]==0) ? OFFSET-amp : OFFSET;
    }

    k_viterbi(&metric, obuf, symbols, bits, mettab, 0, 0);

    *olen = bits;
    return 0;
}

#endif
